// 要将代码从两个传感器扩展到三个传感器，主要需要对几个地方进行修改：观测矩阵 H、观测值矩阵 z 和噪声协方差矩阵 R。以下是具体的修改步骤：
// 更新观测矩阵 H： 将 H 的行数从2增加到3，以适应三个传感器的观测。
// 更新观测值矩阵 z： 将 z 的行数从2增加到3，以存储三个传感器的观测值。
// 更新噪声协方差矩阵 R： 将 R 从2x2矩阵扩展到3x3矩阵，以考虑三个传感器的噪声。
// 初始化新的传感器数据： 增加第三个传感器的测量值，并将其填充到新的观测矩阵 z 中。
#include <iostream>
#include <Eigen/Dense>
#include <vector>
#include "kalmanfilter.h"

int main() {
    // 设飞行器相对雷达做匀加速直线运动
    int n = 10; // 观测点数

    // 初始化变量
    Eigen::VectorXd x(3);
    x << 0, 0, 0.3;

    Eigen::MatrixXd p = Eigen::MatrixXd::Zero(3, 3);
    p(0, 0) = 8;
    p(1, 1) = 10;
    p(2, 2) = 15;
    Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(3, 3);
    Eigen::MatrixXd R = Eigen::MatrixXd::Zero(3, 3); // 三个传感器的噪声协方差矩阵
    R(0, 0) = 0.15;
    R(1, 1) = 0.15;
    R(2, 2) = 0.15;
    Eigen::MatrixXd F(3, 3);
    F << 1, 2, 2,
         0, 1, 2,
         0, 0, 1;

    Eigen::MatrixXd H(3, 3);
    H << 1, 0, 0,
         1, 0, 0,
         1, 0, 0; // 扩展到三个传感器

    // 观测轨迹
    Eigen::MatrixXd z(3, n);
    std::vector<double> z_temp1 = {0.36, 1.56, 3.64, 6.44, 10.5, 14.8, 20.0, 25.2, 32.2, 40.4};
    std::vector<double> z_temp2 = {0.35, 1.55, 3.65, 6.40, 10.45, 14.75, 20.05, 25.15, 32.25, 40.45}; // 第二个传感器的测量值
    std::vector<double> z_temp3 = {0.37, 1.57, 3.66, 6.42, 10.48, 14.78, 20.02, 25.18, 32.28, 40.50}; // 第三个传感器的测量值
    
    for (int i = 0; i < n; ++i) {
        z(0, i) = z_temp1[i]; // 第一个传感器的观测值
        z(1, i) = z_temp2[i]; // 第二个传感器的观测值
        z(2, i) = z_temp3[i]; // 第三个传感器的观测值
    }

    KalmanFilter kalmanfilter(x, p, Q, R, F, H);

    // 保存所有的状态向量
    Eigen::MatrixXd x_kalman_state(3, n+1);
    x_kalman_state.col(0) = x;
    Eigen::MatrixXd kalman_covariance(3, n);

    // 对每一列观测值进行Kalman滤波
    for (int i = 0; i < n; ++i) {
        Eigen::VectorXd z_col = z.col(i);
        KalmanFilterResult kalm_result = kalmanfilter.filter(z_col);
        x_kalman_state.col(i+1) = kalm_result.state;
        kalman_covariance = kalm_result.covariance;
    }

    // 打印结果
    std::cout << "Kalman Filtered States:\n" << x_kalman_state << std::endl;
    std::cout << "Kalman Filtered Covariance:\n" << kalman_covariance << std::endl;

    return 0;
}
